import time
from pyb import Timer, Pin
class userServo:
	tim4 = None
	tim4channel = None
	tim4Freq = None
	curAngle = 90
	def __init__(self, Tim4Channel, Px, timxFreq):
		self.tim4Freq = timxFreq
		self.tim4 = Timer(4, freq = timxFreq)
		self.tim4channel = self.tim4.channel(Tim4Channel, self.tim4.PWM, pin = Pin(Px), \
			pulse_width_percent = self.angleToPercent(self.curAngle))
		time.sleep(500)
		return
	def servoAngle(self, dst_angle):
		self.tim4channel.pulse_width_percent(self.angleToPercent(dst_angle))
		self.DelayMoveTime(dst_angle)
		self.curAngle = dst_angle
		return
	def DelayMoveTime(self, dst_angle):
		time.sleep(int(abs(dst_angle-self.curAngle)*1000/180))
	def Angle(self):
		return self.curAngle
	def angleToPercent(self, servo_angle):
		pul_w = 0.0005+servo_angle/180*0.002
		pul_w_perc = pul_w*self.tim4Freq*100
		return pul_w_perc
if __name__ == '__main__':
	servo1 = userServo(1, 'P7', 50)
	while(True):
		servo1.servoAngle(0)
		servo1.servoAngle(120)